/*
 * str.c
 *
 *  Created on: Oct 19, 2024
 *      Author: 35702
 */
#include "main.h"
#include "stm32h7xx_hal.h"
#include "tim.h"
#include "math.h"
#include "str.h"
#include "stdlib.h"
unsigned int flag=1;
unsigned int MaiChong=100;
unsigned int ChongZhong=0;
Motor_Config_p motor_cfg[7];  /*结构体数组存放相关数据*/
//#define  A_T_x10   ((float)(10* (168000000 / 84 )* (1))) //10 * 步距角  *  计数频率  2M的计数频率

#define TIM_FREQ            480000000U                      /* 定时器主频 */
#define MAX_STEP_ANGLE      0.225                           /* 最小步距(1.8/MICRO_STEP) */
#define PAI                 3.1415926                       /* 圆周率*/
#define FSPR                200                             /* 步进电机单圈步数 */
#define MICRO_STEP          32                               /* 步进电机驱动器细分数 */
#define T1_FREQ             (TIM_FREQ/180)                   /* 频率ft值 */
#define SPR                 (FSPR*MICRO_STEP)               /* 旋转一圈需要的脉冲数 */

/* 数学常数 */

#define ALPHA               ((float)(2*PAI/SPR))            /* α = 2*pi/spr */
#define A_T_x10             ((float)(10*ALPHA*T1_FREQ))
#define T1_FREQ_148         ((float)((T1_FREQ*0.69)/10))    /* 0.69为误差修正值 */
#define A_SQ                ((float)(2*100000*ALPHA))
#define A_x200              ((float)(200*ALPHA))            /* 2*10*10*a/10 */





#define Ft    2000000    // 计数频率 2M
#define off   0
#define  on 1
unsigned int start_motor=1;
#define RUN  0
#define ACCEL  1
#define DECEL  -1
#define STOP_start  8988
#define STOP  250


void CH1_Motor_PWM(Motor_Config_p * list);
void elrs_out(Motor_Config_p * list,TIM_HandleTypeDef *htim, uint32_t channel) ;




// 使用说明
/*

Get_Motor_1(1,setp,400,400,900,&htim1,TIM_CHANNEL_2);


将中断判断条件放入中断中

*/



void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{

    if(htim == &htim1 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
        CH1_Motor_PWM(&motor_cfg[0]);
    }
    if(htim == &htim1 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) {
        CH1_Motor_PWM(&motor_cfg[1]);
    }

    if(htim == &htim1 && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_3) {
        CH1_Motor_PWM(&motor_cfg[2]);
    }
}


/*电机初始化将电机的计数值全部归0--*/
void Motor_Init()
{
    for(int i=0;i<6;i++)
    {
        motor_cfg[i].ZhuangTai=STOP;
    }

}



void CH1_Motor_PWM(Motor_Config_p * list)
{

    list->look=1 ;  // 锁住当前状态
    TIM_HandleTypeDef *htim;
    htim=list->htim;
    uint32_t channel = list->channel;

    __IO uint32_t tim_count = 0;
    __IO uint32_t tmp = 0;



    /*读取定时器的值*/
    tim_count = __HAL_TIM_GET_COUNTER(htim);
    tmp = tim_count + list->step_delay / 2; /*反转两次为一个*/
    __HAL_TIM_SET_COMPARE(htim, channel, (tmp));  /*给通道数值*/


    list->i++;
    if ( list->i == 2) // 一个脉冲
    {
        list->i = 0;
        switch (list->ZhuangTai) // 状态机函数
        {
            /* ------------------------------------------------------------------- 停止 ----------------------------------------------------------------------------*/
            case STOP_start:
                list->step_count = 0;
                HAL_TIM_OC_Stop_IT(htim, channel);

                list->new_step_delay=0;
                list->step_count=0;
                list->accel_count=0;

                list->last_accel_delay = 0;
                list->step_count = 0;
                list->rest = 0;
                list->i = 0;
                list->ZhuangTai = STOP;
                list->look=0 ;  //解锁

                break;
                /* -------------------------------------------------------------------  加速  ----------------------------------------------------------------------------*/
            case ACCEL:
                list->step_count++;
                list->accel_count++;
                list->new_step_delay = (list->step_delay - (((2 * list->step_delay) + list->rest) / (4 * list->accel_count + 1)));
                list->rest = ((2 * list->step_delay) + list->rest) % (4 * list->accel_count + 1);
                if (list->new_step_delay <= list->MIN_C0)
                {
                    list->last_accel_delay = list->new_step_delay;
                    list->new_step_delay = list->MIN_C0;
                    list->rest = 0;
                    list->ZhuangTai = RUN;
                }
                else if (list->step_count >= list->decel_start)
                {
                    list->accel_count = list->decel_lim;
                    list->ZhuangTai = DECEL;
                }
                break;
                /* -------------------------------------------------------------------  匀速  ----------------------------------------------------------------------------*/
            case RUN:
                list->step_count++;
                if (list->step_count >= list->decel_start)
                {
                    list->accel_count = list->decel_lim;
                    list->ZhuangTai = DECEL;
                }
                break;
                /* -------------------------------------------------------------------  减速  ----------------------------------------------------------------------------*/
            case DECEL:
                list->step_count++;
                list->accel_count++;

                list->new_step_delay = list->step_delay + (((2 * list->step_delay) + list->rest) / (4 * abs(list->accel_count) + 1));
                list->rest = ((2 * list->step_delay) + list->rest) % (4 * abs(list->accel_count) + 1);

                if (list->accel_count>=-1000)
                {
                    list->i=0;
                }
                if (list->accel_count >= 0)
                {
                    list->ZhuangTai = STOP_start;
                }

                if(list->step_count >=list->sum_step)
                {
                    list->ZhuangTai = STOP_start;
                }

                break;
        }
        list->step_delay = list->new_step_delay;
    }
}


// 生成梯形加减速的代码
Motor_Config_p * Get_Motor_1(int motor_num ,unsigned long step,unsigned long accel ,unsigned int decel , unsigned int max_speed ,TIM_HandleTypeDef *htim,uint32_t channel)
{

    if (motor_num<0  ||  motor_num>6)
        return NULL;


    motor_cfg[motor_num].V_max =max_speed;
    motor_cfg[motor_num].w1  = accel;
    motor_cfg[motor_num].w2  = decel;
    __IO uint16_t tim_count;        /* 达到最大速度时的步数*/
    __IO uint32_t max_s_lim;        /* 必须要开始减速的步数（如果加速没有达到最大速度）*/
    __IO uint32_t accel_lim;        // 加速段的步数
    __IO int32_t decel_lim;        // 加速段的步数
    __IO uint32_t decel_lim_start;    /*开始减速的步数 -- 梯形加减速时*/


    if( motor_cfg[motor_num].ZhuangTai!=STOP)  // 电机只有在停止的时候才进行加减速,否则退出
        return NULL ;


    // 步数正负判断先省略,默认为正

    if(step ==1 ) // 进入减速状态
    {
        motor_cfg[motor_num].ZhuangTai=DECEL; // 进入减速状态
        motor_cfg[motor_num].step_count=250;  //设置默认的速度
    }
    else if(step !=0)  // 运动步数不为0

        // 设置最小的计数值,及最大速度
        motor_cfg[motor_num].MIN_C0  = (int32_t)(A_T_x10 / motor_cfg[motor_num].V_max);
    // 计算第一个c0值来设计加速度, 单位为 0.1 弧度
    // 计算出c0的值
    motor_cfg[motor_num].step_delay = (uint32_t)((T1_FREQ_148  * sqrt(A_SQ   / accel  )) / 10 );
    // 计算最大的速度所对应的步数
    max_s_lim = (uint32_t )( motor_cfg[motor_num].V_max *  motor_cfg[motor_num].V_max  /(A_x200* accel /10));

    if(max_s_lim<=0)
        max_s_lim=1;

    accel_lim = (uint32_t)(step*decel/(accel+decel));
    if(accel_lim <= max_s_lim)
    {
        /*三角形图像*/
        /* accel_lim为加速段步数*/
        /*减速段步数*/
        decel_lim = -(step-accel_lim);   /*减速段是-的*/
        if(decel_lim>=0)
            decel_lim=-1  ;
    }
    else
    {
        /*梯形图像*/
        /* max_s_lim为加速段步数*/
        decel_lim = -(max_s_lim * accel /decel);  /*减速段为复数*/
        if (decel_lim>=0)     /*设置最小值*/
            decel_lim=-1;
    }
    /*计算出 开始 减速的步数*/
    motor_cfg[motor_num].decel_start =step + decel_lim;
    motor_cfg[motor_num].decel_lim = decel_lim ;        // 传入减速段的步数

    motor_cfg[motor_num].sum_step=step;
    if ( motor_cfg[motor_num].step_delay <=  motor_cfg[motor_num].MIN_C0 ) /*如果一开始c0的速度比匀速时 还小*/
    {
        motor_cfg[motor_num].step_delay =   motor_cfg[motor_num].MIN_C0;
        motor_cfg[motor_num].ZhuangTai =   RUN ;  /*进入匀速*/
    }
    else
    {
        motor_cfg[motor_num].ZhuangTai = ACCEL;   /*加速状态*/
    }


//    while(zhuang_tai);

    motor_cfg[motor_num].new_step_delay;
    motor_cfg[motor_num].last_accel_delay ;
    motor_cfg[motor_num].step_count_2;
    motor_cfg[motor_num].rest ;
    motor_cfg[motor_num].i ;

    motor_cfg[motor_num].htim=htim;
    motor_cfg[motor_num].channel=channel;
HAL_TIM_OC_Start_IT(htim, channel);


    return  & motor_cfg[motor_num];
}


//void elrs_out(Motor_Config_p * list,TIM_HandleTypeDef *htim, uint32_t channel) {
//
//    uint16_t  tmps = __HAL_TIM_GET_COUNTER(htim) +  list->ZhuangTai; /*反转两次为一个*/
//    __HAL_TIM_SET_COMPARE(htim, channel, (tmps));  /*给通道数值*/
//}
